﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using MissionPlanner.Comms;
using MissionPlanner.Utilities;

namespace MissionPlanner
{
    public partial class SerialOutputMD : Form
    {
        System.Threading.Thread t12;
        static bool threadrun = false;
        static internal SerialPort comPort = new SerialPort();
        static internal PointLatLngAlt HomeLoc = new PointLatLngAlt(0, 0, 0, "Home");

        public SerialOutputMD()
        {
            InitializeComponent();

            CMB_serialport.DataSource = SerialPort.GetPortNames();

            if (threadrun)
            {
                BUT_connect.Text = Strings.Stop;
            }

            MissionPlanner.Utilities.Tracking.AddPage(this.GetType().ToString(), this.Text);
        }

        private void BUT_connect_Click(object sender, EventArgs e)
        {
            if (comPort.IsOpen)
            {
                threadrun = false;
                comPort.Close();
                BUT_connect.Text = Strings.Connect;
            }
            else
            {
                try
                {
                    comPort.PortName = CMB_serialport.Text;
                }
                catch
                {
                    CustomMessageBox.Show(Strings.InvalidPortName);
                    return;
                }
                try
                {
                    comPort.BaudRate = int.Parse(CMB_baudrate.Text);
                }
                catch
                {
                    CustomMessageBox.Show(Strings.InvalidBaudRate);
                    return;
                }
                try
                {
                    comPort.Open();
                }
                catch
                {
                    CustomMessageBox.Show("Error Connecting\nif using com0com please rename the ports to COM??");
                    return;
                }

                t12 = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop))
                {
                    IsBackground = true,
                    Name = "Nmea output"
                };
                t12.Start();
            }
        }

        void mainloop()
        {
            threadrun = true;
            int counter = 0;
            while (threadrun)
            {
                try
                {
                    //http://www.microdrones.com/wiki/index.php/Downlink_Protocol

                    /*
...
#1,27,48,1,1,1,1,0,25343,8192,85
#2,1,0,0,0,-100,-100,0,-100,-100,50,0,0,0,100,167
#3,0,0,0,0,13
#4,9,111731750,1595,0,197
#5,398843292,55871844,492974923,2.240,9,5
#6,0.15,-0.06,0.21,1.0,197
#7,-0.042,-0.022,2.474,182
#8,0.000,-418.438,2158,159
#9,-139,-139,431,199
#10,1.496,-1.628,0.842,161
#11,0,0,0,58
#12,,33
#1,27,48,1,1,1,1,0,25343,8192,85
#2,1,0,0,0,-100,-100,0,-100,-100,50,0,0,0,100,167
#3,0,0,0,0,13
#4,9,111732000,1595,0,208
#5,398843288,55871840,492974925,2.240,9,2
#6,0.07,-0.02,0.16,0.9,188
#7,-0.042,-0.019,2.475,175
#8,0.000,-418.233,2158,166
#9,-139,-139,431,199
#10,1.354,-1.549,0.972,162
#11,0,0,0,58
#12,,33
...

                     */

                    writeline("#1,28,07,2,1,1,1,2,16000,0,2,");

                    //   writeline("#2,100,99,98,97,96,95,94,93,92,");

                    // writeline("#3,100,120,140,160,");

                    int week, seconds;
                    GetGPSTime(out week, out seconds);

                    writeline(string.Format("#4,{0},{1},{2},{3},", counter/10, seconds, week, 25));

                    //    writeline("#4,counter/2,111732000,1595,0,");

                    double x, y, z;

                    GetGeo(out x, out y, out z, MainV2.comPort.MAV.cs.lat, MainV2.comPort.MAV.cs.lng,
                        MainV2.comPort.MAV.cs.alt);

                    writeline(string.Format("#5,{0},{1},{2},{3},{4},", x*100, y*100, z*100,
                        MainV2.comPort.MAV.cs.gpshdop + 0.01, MainV2.comPort.MAV.cs.satcount));

                    writeline(string.Format("#6,{0},{1},{2},{3},",
                        MainV2.comPort.MAV.cs.groundspeed*Math.Sin(MainV2.comPort.MAV.cs.groundcourse*MathHelper.deg2rad),
                        MainV2.comPort.MAV.cs.groundspeed*Math.Cos(MainV2.comPort.MAV.cs.groundcourse*MathHelper.deg2rad),
                        MainV2.comPort.MAV.cs.verticalspeed, 2));

                    writeline(string.Format("#7,{0},{1},{2},", MainV2.comPort.MAV.cs.roll*MathHelper.deg2rad,
                        MainV2.comPort.MAV.cs.pitch*MathHelper.deg2rad, MainV2.comPort.MAV.cs.yaw*MathHelper.deg2rad));

                    writeline(string.Format("#8,{0},{1},{2},", MainV2.comPort.MAV.cs.alt, MainV2.comPort.MAV.cs.alt,
                        MainV2.comPort.MAV.cs.press_temp));

                    writeline(string.Format("#9,{0},{1},{2},", MainV2.comPort.MAV.cs.mx, MainV2.comPort.MAV.cs.my,
                        MainV2.comPort.MAV.cs.mz));

                    //  writeline(string.Format("#10,{0},{1},{2},", 1.354,-1.549,0.972));

                    System.Threading.Thread.Sleep(100);
                    counter++;
                }
                catch
                {
                }
            }
        }

        DateTime GetFromGps(int weeknumber, int seconds)
        {
            DateTime datum = new DateTime(1980, 1, 6, 0, 0, 0, DateTimeKind.Utc);
            DateTime week = datum.AddDays(weeknumber*7);
            DateTime time = week.AddSeconds(seconds);
            return time;
        }

        void GetGPSTime(out int weeknumber, out int seconds)
        {
            DateTime datum = new DateTime(1980, 1, 6, 0, 0, 0, DateTimeKind.Utc);

            TimeSpan ts = DateTime.Now - datum;

            weeknumber = ((int) ts.TotalDays/7);

            ts = DateTime.Now - GetFromGps(weeknumber, 0);

            seconds = (int) ts.TotalSeconds;
        }


        void GetDegrees(double x, double y, double z, out double latitude, out double longitude)
        {
            double deg = 0.01745329252;
            double r = Math.Sqrt(x*x + y*y);
            longitude = Math.Asin(y/r)/deg;
            if (longitude > 180d)
                longitude = 180d - longitude;

            latitude = Math.Atan(z/r)/deg;
        }

        void GetGeo(out double x, out double y, out double z, double latitude, double longitude, double alt)
        {
            double wgs84a = 6378137;
            double wgs84f = 1.0/298.257223563;
            double wgs84b = wgs84a*(1.0 - wgs84f);

            double clat = Math.Cos(MathHelper.deg2rad*latitude);
            double slat = Math.Sin(MathHelper.deg2rad*latitude);
            double clon = Math.Cos(MathHelper.deg2rad*longitude);
            double slon = Math.Sin(MathHelper.deg2rad*longitude);

            var ecc = Math.Sqrt(2*wgs84f - Math.Pow(wgs84f, 2));
            var esq = ecc*ecc;

            alt = alt*0.0001;

            // var rrnrm  = radcur (flat);
            var rn = wgs84a; /// (Math.Sqrt(1-esq)*(slat*slat));// rrnrm[1];
            var re = wgs84a; // rrnrm[0];

            x = (rn + alt)*clat*clon;
            y = (rn + alt)*clat*slon;
            z = ((1 - esq)*rn + (alt))*slat;
        }

        void writeline(string line)
        {
            comPort.Write(line + checksum(line) + "\r\n");
        }

        byte checksum(string line)
        {
            byte ans = 0;
            foreach (char ch in line.ToCharArray())
            {
                ans += (byte) ch;
            }
            ans = (byte) (ans ^ 0xff);

            return ans;
        }

        private void SerialOutput_FormClosing(object sender, FormClosingEventArgs e)
        {
        }
    }
}